/**
  ******************************************************************************
  * @file           : auto_patrol_task.cpp
  * @author         : Tessia
  * @brief          : None
  * @attention      : None
  * @date           : 2024/3/13
  ******************************************************************************
  */



#include "auto_patrol_task.h"


extern rc_device_t g_rc;
extern float yaw_gyro;
extern float yaw_set_gyro;
extern chassis_yaw_info chassis_yaw_cal;
extern judge_info_0 judgeInfo0;


float deg2float(float deg){
    return(deg / 360.f);
}

float compare_abs_min(float A, float B){
    if(abs(A) < abs(B)){
        return(A);
    }else{
        return(B);
    }
}

float optimal_solution_cal(float target, float current){
    float round_target1 = floor(current) + target;
    float round_target2 = floor(current) + target - 1;
    float round_target3 = floor(current) + target + 1;

    float rounds_target = compare_closest(compare_closest(round_target1, round_target2, current), round_target3, current);
    return(rounds_target);
}

float yaw_set_gyro_filtered = 0;
int8_t yaw_rotate_sense = 1;
uint32_t yaw_rotate_time_cnt = 0;
uint8_t first_rotate = 1;
RC_SW sw_l_last = RC_SW_NONE;
void auto_patrol_task(void *argument){
    for(;;){
        if(yaw_rotate_time_cnt >= 2000 * first_rotate){
            yaw_rotate_time_cnt = 0;
            yaw_rotate_sense = -yaw_rotate_sense;
            first_rotate = 2;
        }

        if(g_rc.is_lost == 0 && g_rc.rc_info.sw.sw_r == RC_SW_UP && g_rc.rc_info.sw.sw_l == RC_SW_MID && judgeInfo0.current_HP != 0){
            first_rotate = sw_l_last == RC_SW_UP ? 1 : first_rotate;
            if(chassis_yaw_cal.is_target_detected == 0 && chassis_yaw_cal.is_chassis_yaw_stop == 0){
                yaw_set_gyro = yaw_set_gyro + 2e-4 * yaw_rotate_sense;
                yaw_rotate_time_cnt ++;
            }
        }else{
            yaw_rotate_time_cnt = 0;
        }


        if(chassis_yaw_cal.is_target_detected == 1 && chassis_yaw_cal.is_chassis_yaw_stop == 0){
            yaw_set_gyro_filtered = optimal_solution_cal(chassis_yaw_cal.chassis_yaw / 360.f - floor(chassis_yaw_cal.chassis_yaw / 360.f), yaw_gyro);
            low_pass(yaw_set_gyro, yaw_set_gyro_filtered, 0.005);
            yaw_rotate_time_cnt = 0;
        }

        sw_l_last = g_rc.rc_info.sw.sw_l;
        osDelay(1);
    }
}
